#include <opencv2/opencv.hpp>
#include <iostream> 
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <csignal>
#include <pcl/io/pcd_io.h>          //I/O操作头文件
#include <pcl/point_types.h>        //点类型定义头文件
// #define BOOST_TYPEOF_EMULATION  //要加在#include <pcl/registration/icp.h>前
#include <pcl/registration/icp.h>   //ICP配准类相关头文件
// #include <pcl/visualization/cloud_viewer.h>//点云查看窗口头文件
#include <pcl/visualization/pcl_visualizer.h>//可视化头文件
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>


using namespace cv;
using namespace std;

Eigen::Matrix3d K;



bool flag = true;
bool camera_flag=false;
ros::Publisher cloud_visual_pub2;
void shutdown_handler(int)
{
    cout<<"shutdown"<<endl;
    flag = false;
} 

void watchSignal() {
    signal(SIGINT, shutdown_handler);
    signal(SIGTERM, shutdown_handler);
    signal(SIGKILL, shutdown_handler);
    signal(SIGQUIT, shutdown_handler);
}

void cameraCallback(sensor_msgs::CameraInfo msg)
{
	// ROS_INFO("ASSSSSSSSSSSSSSS");
	Eigen::Matrix3f tmp;
	tmp<<msg.K[0],msg.K[1],msg.K[2],
		msg.K[3],msg.K[4],msg.K[5],
		msg.K[6],msg.K[7],msg.K[8];
	K=tmp.cast<double>();
	camera_flag=true;


}

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  // ...
	if(!camera_flag)return;
	// ROS_INFO("AAAAAAAAA");
	// cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);


	// K<<350.48,0,320.5,
	// 0,350.48,240.5,
	// 0,0,1;
	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg);

	Mat img=cv_ptr->image;
	double depth_scale=1;
	double Zc=depth_scale*img.at<float>(2,2);
	// ROS_INFO("ZC %f",Zc);
	double cx=320;
    double cy=240;
	double hfov=1.02974;
	double width=480;
	double fx=(width/2.0)/tan(hfov/2);
	double fy=fx;
	// ROS_INFO("fx: %f",fx);



	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);

	for (int m = 0; m < img.rows; m++)
	{
		for (int n=0; n < img.cols; n++)
		{
			double d = img.at<float>(m,n);
			// if (d == 0)
			// continue;
			if(isnan(d))
			{
				// ROS_INFO("NAN");
				d=10;
			}
			pcl::PointXYZ p;

			double Zc=depth_scale*d;
			// Eigen::Vector4d cor((n - cx) * Zc / fx,(m - cy) *Zc / fy,Zc,1);
			// Eigen::Vector4d trans_cor=cor;

			// p.x = trans_cor(0);
			// p.y = trans_cor(1);
			// p.z = trans_cor(2);


			Eigen::Vector3d pixel(n,m,1);
			Eigen::Vector3d coord=Zc*K.inverse()*pixel;

			p.x=coord(0);
			p.y=coord(1);
			p.z=coord(2);

			cloud1->points.push_back(p);
		}
	}

    // convert to ROS message and publish
    sensor_msgs::PointCloud2 cloud_visual;
    pcl::toROSMsg(*cloud1, cloud_visual);

    // message publish should have the same time stamp
    cloud_visual.header.stamp = ros::Time::now();
    cloud_visual.header.frame_id = "iris/camera_link";
    cloud_visual_pub2.publish(cloud_visual);


    // int width=img.cols;
    // int height=img.rows;
    // int channels=img.channels();
	// // ROS_INFO("get depth image");
    // cout<<"get image: "<<"width: "<<width<<" height: "<<height<<" channels: "<<channels<<endl;

}

int main(int argc, char** argv) 
{
    ros::init(argc, argv, "convert_pointcloud");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);

	image_transport::Subscriber depth_sub = it.subscribe("/iris/camera/depth/image_raw", 1, imageCallback);
	cloud_visual_pub2=nh.advertise<sensor_msgs::PointCloud2>("/convert_pointcloud", 1, true);           //map cloud
	ros::Subscriber camera_sub = nh.subscribe("/iris/camera/depth/camera_info", 1, cameraCallback);
    ros::Rate loop_rate(30);

    Mat frame;
    int last_time = 0;
    double t;
    double fps;
    int i = 0;
    char disp_fps[10];
    string filename = "";	
    while (1) 
	{
        // t = (double)cvGetTickCount();
        // if (waitKey(5) == 27) {
        //     break;
        // }
        // watchSignal();

        // if (capture.isOpened())
        // {
        //     capture >> frame;
		// 	Mat grey;

		// 	cvtColor(frame,grey,CV_BGR2GRAY);
		// 	int width=grey.cols;
		// 	int height=grey.rows;
		// 	int channels=grey.channels();
		// 	cout<<"width: "<<width<<" height: "<<height<<" channels: "<<channels<<endl;

        //     sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", grey).toImageMsg();
        //     t = ((double)cv::getTickCount() - t) / cv::getTickFrequency();
        //     fps = 1.0 / t;

        //     sprintf(disp_fps, "%.2f", fps);      // 帧率保留两位小数
        //     string fpsString("FPS:");
        //     fpsString += disp_fps;                    // 在"FPS:"后加入帧率数值字符串

        //     pub.publish(msg);
        //     ros::spinOnce();
        //     loop_rate.sleep();
        // }



		ros::spinOnce();
		loop_rate.sleep();


    }
    cv::destroyAllWindows();
    waitKey(0);
    return 0;
}